﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Drawing;
using System.Text.RegularExpressions;
using System.Runtime.InteropServices;

namespace System
{
	public class AlgebraicException : Exception
	{
		public AlgebraicException()
			: base("Algebraic error")
		{
		}
	}
	public partial struct Quaternion
	{
		public V3D Axis;
		public double Angle;
		public Quaternion(V3D v, double a)
		{
			Axis = v;
			Angle = a;
		}
		public double[] ToMatrix()
		{
			double[] Res = new double[16];
			double c = Math.Cos(Angle);
			double s = Math.Sin(Angle);
			Res[0] = Axis.X * Axis.X + (Axis.Y * Axis.Y + Axis.Z * Axis.Z) * c;
			Res[1] = Axis.X * Axis.Y * (1 - c) - Axis.Z * s;
			Res[2] = Axis.X * Axis.Z * (1 - c) + Axis.Y * s;
			Res[3] = 0;
			Res[4] = Axis.Y * Axis.X * (1 - c) + Axis.Z * s;
			Res[5] = Axis.Y * Axis.Y + (Axis.X * Axis.X + Axis.Z * Axis.Z) * c;
			Res[6] = Axis.Y * Axis.Z * (1 - c) - Axis.X * s;
			Res[7] = 0;
			Res[8] = Axis.Z * Axis.X * (1 - c) - Axis.Y * s;
			Res[9] = Axis.Z * Axis.Y * (1 - c) + Axis.X * s;
			Res[10] = Axis.Z * Axis.Z + (Axis.X * Axis.X + Axis.Y * Axis.Y) * c;
			Res[11] = 0;
			Res[15] = 1;
			return Res;
		}
	}
	public partial class Orientation3D
	{
		public class LocalForce
		{
			public bool Active;
			public V3D Point;
			public V3D Force;
			public LocalForce(V3D P)
			{
				Active = false;
				Point = P;
				Force = V3D.Black;
			}
			public LocalForce(V3D P,V3D F)
			{
				Active = true;
				Point = P;
				Force = F;
			}
			public void SetForce(V3D F)
			{
				Active = true;
				Force = F;
			}
			public void Off()
			{
				Active = false;
			}
		}

		public B_Matrix Q;
		public double Mass;
		public V3D Position = V3D.Black;

		public V3D Velocity = V3D.Black;
		public V3D Moment = V3D.Black;
		public V3D Acc = V3D.Black;

		public Orientation3D(double m)
		{
			Q = B_Matrix.Identity(4);
			Mass = m;
		}

		public List<KeyValuePair<V3D, V3D>> GlobalForces = new List<KeyValuePair<V3D, V3D>>();
		public List<LocalForce> LocalForces = new List<LocalForce>();

		public void ClearForces()
		{
			GlobalForces.Clear();
			LocalForces.Clear();
		}
		public void AddGlobalForce(V3D Point, V3D Force)
		{
			GlobalForces.Add(new KeyValuePair<V3D, V3D>(Point, Force));
		}
		public LocalForce AddLocalForce(V3D Point, V3D Force)
		{
			var f = new LocalForce(Point);
			f.Force = Force;
			LocalForces.Add(f);
			return f;
		}
		public void AddTorque(V3D p, V3D axis, double amount)
		{
			AddLocalForce(p, (axis % p).Unit * amount);
			AddLocalForce(-p, (p % axis).Unit * amount);
		}
		public void ApplyGravity(bool v3g)
		{
			if (v3g)
				AddGlobalForce(Position, Position.Unit * -10 * Mass);
			else
				AddGlobalForce(Position, V3D.Green * -10 * Mass);
		}
		public void Process(double dt)
		{
			Acc = V3D.Black;
			V3D Torque = V3D.Black;
			GlobalForces.ForEach(x =>
			{
				Acc += x.Value;
				Torque += (x.Key - Position) % x.Value;
			});
			LocalForces.ForEach(x =>
			{
				Acc += UnProjectDir(x.Force);
				Torque += UnProjectDir(x.Point % x.Force);
			});
			Acc *= 1 / Mass;
			Velocity += Acc * dt;
			Moment += Torque * dt;
			//Console.WriteLine(Acc + " " + Torque);
			if (Moment.Length > 0)
			{
				var I = B_Matrix.Rotation3D(Moment.Unit, Moment.Length * dt);
				Q = I * Q;
			}
			if (Velocity.Length > 0)
				Position += Velocity * dt;
		}

		public V3D Project(V3D point)
		{
			return Q.Inverse() * point;
		}
		public V3D UnProjectPoint(V3D point)
		{
			return Q * point + Position;
		}
		public V3D UnProjectDir(V3D point)
		{
			return Q * point;
		}
	}

	public class Circle3D
	{
		public V3D Center;
		public V3D Normal;
		public double Radius
		{
			get
			{
				return Normal.Length;
			}
			set
			{
				Normal = Normal.Unit * value;
			}
		}


		public static V3D operator *(Circle3D C, Line3D L)
		{
			return C.Center;
		}
		public V3D _IntersectLine(Line3D L)
		{
			return V3D.Null;
		}
	}
	public class Sphere3D
	{
		public V3D Center;
		public double Radius;
		public Sphere3D(V3D c, double r)
		{
			Center = c;
			Radius = r;
		}
		public Sphere3D(double r)
			: this(V3D.Black, r)
		{
		}
		public static Line3D operator *(Sphere3D s, Line3D l)
		{
			V3D M = l.DistanceFromPoint(s.Center);
			if (M.Length > s.Radius)
				return Line3D.Null;
			else
			{
				double d = Math.Sqrt(s.Radius * s.Radius - M.Length * M.Length);
				return new Line3D(M - l.Dir * d, M + l.Dir * d);
			}
		}
	}
	public class Beam3D
	{
		public V3D Base;
		public V3D Dir;
		public double Radius;
		public Beam3D(V3D b, V3D d, double r)
		{
			Base = b;
			Dir = d;
			Radius = r;
		}
	}
	public class Cylinder3D//Cylinder
	{
		public V3D Base;
		public V3D Top;
		public double Radius;
		public Line3D Axis
		{
			get
			{
				return new Line3D(Base, Top);
			}
		}
		public V3D Normal
		{
			get
			{
				return (Top - Base).Unit;
			}
		}
		public Cylinder3D(V3D B, V3D T, double R)
		{
			Base = B;
			Top = T;
			Radius = R;
		}
		public static implicit operator Beam3D(Cylinder3D C)
		{
			return new Beam3D(C.Base, C.Normal, C.Radius);
		}

		public V3D NearestPoint(V3D p)
		{
			return new Line3D(Base, Base + Normal).NearestPoint(p);
		}
		public V3D NearestPoint_Seg(V3D p)
		{
			return new Line3D(Base, Base + Normal).NearestPoint_Seg(p);
		}
	}

	//Math functions
	public static class Algebra3D
	{
		public static double Map(double RangeMin, double RangeMax, double N, double NewMin, double NewMax)
		{
			double x = (N - RangeMin) / (RangeMax - RangeMin);
			return NewMin * (1 - x) + NewMax * x;
		}

		public static V3D Bezier(V3D SP, V3D SN, V3D EN, V3D EP, double n)
		{
			return ((1 - n) * (1 - n) * (1 - n) * SP) + 3 * n * (1 - n) * (1 - n) * SN + 3 * n * n * (1 - n) * EN + EP * n * n * n;
		}
		public static void Resize(double w, double h, double framew, double frameh, out double neww, out double newh)
		{
			if (w / h > framew / frameh)
			{
				neww = framew;
				newh = h / w * framew;
			}
			else
			{
				newh = frameh;
				neww = w / h * frameh;
			}
		}
	}
	public struct Triangle3D
	{
		public V3D A, B, C;
		public bool Valid;
		public Line3D a
		{
			get
			{
				return new Line3D(A, B);
			}
		}
		public Line3D b
		{
			get
			{
				return new Line3D(B, C);
			}
		}
		public Line3D c
		{
			get
			{
				return new Line3D(C, A);
			}
		}
		public V3D Normal
		{
			get
			{
				return ((B - A) % (C - A)).Unit;
			}
		}
		//Constructors
		public Triangle3D(V3D P1, V3D P2, V3D P3)
		{
			A = P1;
			B = P2;
			C = P3;
			Valid = P1.Valid && P2.Valid && P3.Valid;
		}

		public bool PointIn(V3D P)
		{
			V3D PA = P - A;
			V3D PB = P - B;
			V3D PC = P - C;
			if (Normal * ((B - A) % PA) < 0)
				return false;
			else if (Normal * ((C - B) % PB) < 0)
				return false;
			else if (Normal * ((A - C) % PC) < 0)
				return false;
			else
				return true;
		}
		public static implicit operator Plane3D(Triangle3D T)
		{
			return new Plane3D(T.A, T.Normal);
		}
		public void DrawGraphics(Graphics G, Brush br)
		{
			//PointF[] X = new PointF[] { A, B, C };
			//G.FillPolygon(br, X);
		}
		public double[] VBOPN()
		{
			double[] res = new double[18];
			Buffer.BlockCopy(A.ToArray(), 0, res, 0, 3);
			Buffer.BlockCopy(Normal.ToArray(), 0, res, 3, 3);
			Buffer.BlockCopy(B.ToArray(), 0, res, 6, 3);
			Buffer.BlockCopy(Normal.ToArray(), 0, res, 9, 3);
			Buffer.BlockCopy(C.ToArray(), 0, res, 12, 3);
			Buffer.BlockCopy(Normal.ToArray(), 0, res, 15, 3);
			return res;
		}
	}

	//Complex Objects
	public class Poly2D
	{
		public List<List<V3D>> Contours;
		public List<List<V3D>> InBorders;
		public V3D AABBMin;
		public V3D AABBMax;
		public Poly2D()
		{
			Contours = new List<List<V3D>>();
			InBorders = new List<List<V3D>>();
		}
		public Poly2D(List<V3D> SimpleContour)
			: this()
		{
			Contours.Add(SimpleContour);
			GenAABB();
		}
		public Poly2D(List<Line3D> Lines)
			: this()
		{
			int N;
			//Create Rings
			while (Lines.Count > 0)
			{
				List<V3D> NewRing = new List<V3D>();
				NewRing.Add(Lines[0].A);
				NewRing.Add(Lines[0].B);
				Lines.RemoveAt(0);
				N = 0;
				while (NewRing[0] != NewRing[NewRing.Count - 1])
				{
					if (Lines[N].A == NewRing[0])
					{
						NewRing.Insert(0, Lines[N].B);
						Lines.RemoveAt(N);
					}
					else if (Lines[N].B == NewRing[0])
					{
						NewRing.Insert(0, Lines[N].A);
						Lines.RemoveAt(N);
					}
					else if (Lines[N].A == NewRing[NewRing.Count - 1])
					{
						NewRing.Add(Lines[N].B);
						Lines.RemoveAt(N);
					}
					else if (Lines[N].B == NewRing[NewRing.Count - 1])
					{
						NewRing.Add(Lines[N].A);
						Lines.RemoveAt(N);
					}
					else
						N++;
					if (Lines.Count > 0)
						N = N % Lines.Count;
					else
						N = -1;
				}
				NewRing.RemoveAt(0);
				Contours.Add(NewRing);
			}
			//Detect Inborders
			N = 0;
			while (N < Contours.Count)
			{
				bool Detected = false;
				for (int i = 0; i < Contours.Count; i++)
					if (!Detected && i != N)
						Detected |= new Poly2D(Contours[i]).PointIn(Contours[N][0]);
				if (Detected)
				{
					InBorders.Add(Contours[N]);
					Contours.RemoveAt(N);
				}
				else
					N++;
			}
			GenAABB();
		}

		public List<Line3D> GetSides()
		{
			List<Line3D> Result = new List<Line3D>();
			for (int i = 0; i < Contours.Count; i++)
				for (int j = 0; j < Contours[i].Count; j++)
					Result.Add(new Line3D(Contours[i][j], Contours[i][(j + 1) % Contours[i].Count]));
			for (int i = 0; i < InBorders.Count; i++)
				for (int j = 0; j < InBorders[i].Count; j++)
					Result.Add(new Line3D(InBorders[i][j], InBorders[i][(j + 1) % InBorders[i].Count]));
			return Result;
		}

		public Poly2D Clone()
		{
			Poly2D Res = new Poly2D();
			for (int i = 0; i < Contours.Count; i++)
			{
				Res.Contours.Add(new List<V3D>());
				for (int j = 0; j < Contours[i].Count; j++)
					Res.Contours[i].Add(Contours[i][j]);
			}
			for (int i = 0; i < InBorders.Count; i++)
			{
				Res.InBorders.Add(new List<V3D>());
				for (int j = 0; j < InBorders[i].Count; j++)
					Res.InBorders[i].Add(InBorders[i][j]);
			}
			Res.GenAABB();
			return Res;
		}
		public void AddPoint(int C, V3D P)
		{
			Contours[C].Add(P);
			GenAABB();
		}
		public void InsertContourPoint(int ContourIndex, int N, V3D P)
		{
			Contours[ContourIndex].Insert(N, P);
			GenAABB();
		}
		public void InsertBorderPoint(int BorderIndex, int N, V3D P)
		{
			InBorders[BorderIndex].Insert(N, P);
			GenAABB();
		}
		public void CutContourSide(int ContourIndex, int I, double n)
		{
			InsertContourPoint(ContourIndex, I + 1, V3D.Interpolate(Contours[ContourIndex][I], Contours[ContourIndex][(I + 1) % Contours[ContourIndex].Count], n));
		}
		public void CutBorderSide(int BorderIndex, int I, double n)
		{
			InsertBorderPoint(BorderIndex, I + 1, V3D.Interpolate(Contours[BorderIndex][I], Contours[BorderIndex][(I + 1) % Contours[BorderIndex].Count], n));
		}
		public void DrawGraphics(Graphics G, Color C)
		{
			for (int i = 0; i < Contours.Count; i++)
			{
				List<PointF> X = new List<PointF>();
				for (int j = 0; j < Contours[i].Count; j++)
				{
					//X.Add(Contours[i][j]);
					//Contours[i][j].DrawGraphics(G, Brushes.Blue, 0.03);
				}
				//G.DrawPolygon(new Pen(Color.Blue,0.03f), X.ToArray());
			}
			for (int i = 0; i < InBorders.Count; i++)
			{
				List<PointF> X = new List<PointF>();
				for (int j = 0; j < InBorders[i].Count; j++)
				{
					//X.Add(InBorders[i][j]);
					//InBorders[i][j].DrawGraphics(G, Brushes.Red, 3);
				}
				//G.DrawPolygon((new V3D(1, 0, 0) * (i + 1) * 64).ToPen(2), X.ToArray());
			}
		}

		public void GenAABB()
		{
			Line3D[] X = GetSides().ToArray();
			if (X.Length > 0)
			{
				AABBMin = AABBMax = X[0].A;
				for (int i = 0; i < X.Length; i++)
				{
					AABBMin.X = Math.Min(AABBMin.X, Math.Min(X[i].A.X, X[i].B.X));
					AABBMin.Y = Math.Min(AABBMin.Y, Math.Min(X[i].A.Y, X[i].B.Y));
					AABBMax.X = Math.Max(AABBMax.X, Math.Max(X[i].A.X, X[i].B.X));
					AABBMax.Y = Math.Max(AABBMax.Y, Math.Max(X[i].A.Y, X[i].B.Y));
				}
			}
		}
		public bool PointIn(V3D Point)
		{
			if (Point.X >= AABBMin.X && Point.X <= AABBMax.X && Point.Y >= AABBMin.Y && Point.Y <= AABBMax.Y)
			//if(true)
			{
				List<Line3D> S = GetSides();
				V3D VMin = S[0].A;
				for (int i = 0; i < S.Count; i++)
					VMin = new V3D(Math.Min(VMin.X, Math.Min(S[i].A.X, S[i].B.X)), Math.Min(VMin.Y, Math.Min(S[i].A.Y, S[i].B.Y)), 0);
				Line3D L = new Line3D(VMin - new V3D(1, 1, 0), Point);
				int N = 0;
				for (int i = 0; i < S.Count; i++)
				{
					if ((L * S[i]).Valid)
						N++;
				}
				return N % 2 == 1;
			}
			else
				return false;
		}

		public void CutByLine(Line3D L)
		{
			for (int i = 0; i < Contours.Count; i++)
			{
				int N = 0;
				while (N < Contours[i].Count)
				{
					Line3D S = new Line3D(Contours[i][N], Contours[i][(N + 1) % Contours[i].Count]);
					V3D M;
					if (L.A == S.A || L.A == S.B || L.B == S.A || L.B == S.B)
						M = V3D.Null;
					else
						M = L * S;
					if (M.Valid)
					{
						if (M.X.Equals(double.NaN))
							M = L * S;
						InsertContourPoint(i, N + 1, M);
						N++;
					}
					N++;
				}
			}
			for (int i = 0; i < InBorders.Count; i++)
			{
				int N = 0;
				while (N < InBorders[i].Count)
				{
					Line3D S = new Line3D(InBorders[i][N], InBorders[i][(N + 1) % InBorders[i].Count]);
					V3D M;
					if (L.A == S.A || L.A == S.B || L.B == S.A || L.B == S.B)
						M = V3D.Null;
					else
						M = L * S;
					if (M.Valid)
					{
						if (M.X.Equals(double.NaN))
							M = L * S;
						InsertBorderPoint(i, N + 1, M);
						N++;
					}
					N++;
				}
			}
		}
		public void CutByPolygon(Poly2D P)
		{
			Poly2D This = this.Clone();
			Poly2D B = P.Clone();

			List<Line3D> X = P.GetSides();
			for (int i = 0; i < X.Count; i++)
			{
				if (i == 37)
				{
				}
				CutByLine(X[i]);
			}
		}

		public List<Poly2D> GetLineSegments(Poly2D P)
		{
			Poly2D Z1 = Clone();
			Poly2D Z2 = P.Clone();
			Z1.CutByPolygon(P);
			Z2.CutByPolygon(this);
			List<Poly2D> Res = new List<Poly2D>();
			Res.Add(Z1);
			Res.Add(Z2);
			return Res;
		}
		public static Poly2D operator +(Poly2D P1, Poly2D P2)
		{
			Poly2D Z1 = P1.Clone();
			Poly2D Z2 = P2.Clone();
			Z1.CutByPolygon(P2);
			Z2.CutByPolygon(P1);
			List<Line3D> X1 = Z1.GetSides();
			List<Line3D> X2 = Z2.GetSides();
			List<Line3D> Res = new List<Line3D>();
			int N = 0;
			while (N < X1.Count)
			{
				V3D C = (X1[N].A + X1[N].B) / 2;
				if (P2.PointIn(C))
					X1.RemoveAt(N);
				else
					N++;
			}
			Res.AddRange(X1);
			N = 0;
			while (N < X2.Count)
			{
				V3D C = (X2[N].A + X2[N].B) / 2;
				if (P1.PointIn(C))
					X2.RemoveAt(N);
				else
					N++;
			}
			Res.AddRange(X2);
			return new Poly2D(Res);
		}
		public static Poly2D operator -(Poly2D P1, Poly2D P2)
		{
			Poly2D Z1 = P1.Clone();
			Poly2D Z2 = P2.Clone();
			Z1.CutByPolygon(P2);
			Z2.CutByPolygon(P1);
			List<Line3D> X1 = Z1.GetSides();
			List<Line3D> X2 = Z2.GetSides();
			List<Line3D> Res = new List<Line3D>();
			int N = 0;
			while (N < X1.Count)
			{
				V3D C = (X1[N].A + X1[N].B) / 2;
				if (P2.PointIn(C))
					X1.RemoveAt(N);
				else
					N++;
			}
			Res.AddRange(X1);
			N = 0;
			while (N < X2.Count)
			{
				V3D C = (X2[N].A + X2[N].B) / 2;
				if (!P1.PointIn(C))
					X2.RemoveAt(N);
				else
					N++;
			}
			Res.AddRange(X2);
			return new Poly2D(Res);
		}
		public static Poly2D operator *(Poly2D P1, Poly2D P2)
		{
			Poly2D Z1 = P1.Clone();
			Poly2D Z2 = P2.Clone();
			Z1.CutByPolygon(P2);
			Z2.CutByPolygon(P1);
			List<Line3D> X1 = Z1.GetSides();
			List<Line3D> X2 = Z2.GetSides();
			List<Line3D> Res = new List<Line3D>();
			int N = 0;
			while (N < X1.Count)
			{
				V3D C = (X1[N].A + X1[N].B) / 2;
				if (!P2.PointIn(C))
					X1.RemoveAt(N);
				else
					N++;
			}
			Res.AddRange(X1);
			N = 0;
			while (N < X2.Count)
			{
				V3D C = (X2[N].A + X2[N].B) / 2;
				if (!P1.PointIn(C))
					X2.RemoveAt(N);
				else
					N++;
			}
			Res.AddRange(X2);
			return new Poly2D(Res);
		}
		public static Poly2D operator %(Poly2D P1, Poly2D P2)
		{
			Poly2D Z1 = P1.Clone();
			Poly2D Z2 = P2.Clone();
			Z1.CutByPolygon(P2);
			Z2.CutByPolygon(P1);
			List<Line3D> X1 = Z1.GetSides();
			List<Line3D> X2 = Z2.GetSides();
			List<Line3D> Res = new List<Line3D>();
			int N = 0;
			while (N < X1.Count)
			{
				V3D C = (X1[N].A + X1[N].B) / 2;
				N++;
			}
			Res.AddRange(X1);
			N = 0;
			while (N < X2.Count)
			{
				V3D C = (X2[N].A + X2[N].B) / 2;
				N++;
			}
			Res.AddRange(X2);
			return new Poly2D(Res);
		}

		public List<Triangle3D> Triangularizate()
		{
			List<Triangle3D> Res = new List<Triangle3D>();
			return Res;
		}

		public static List<V3D> CommonBorder(Poly2D P1, Poly2D P2)
		{
			return null;
		}
	}

	public class Poly3D
	{
		public List<V3D> Vertices = new List<V3D>();
		public List<Tuple<int, int, int>> Triangles = new List<Tuple<int, int, int>>();
		public List<Tuple<int, int>> Lines = new List<Tuple<int, int>>();

		public Tuple<V3D, V3D, V3D> GetTriangle(int N)
		{
			return new Tuple<V3D, V3D, V3D>(Vertices[Triangles[N].Item1], Vertices[Triangles[N].Item2], Vertices[Triangles[N].Item3]);
		}
		public Poly3D()
		{
		}
		public int AddPoint(V3D P)
		{
			Vertices.Add(P);
			return Vertices.Count - 1;
		}
		public void AddTriangle(int n1, int n2, int n3)
		{
			Triangles.Add(new Tuple<int, int, int>(n1, n2, n3));
		}
		public void AddLine(int n1, int n2)
		{
			Lines.Add(new Tuple<int, int>(n1, n2));
		}
	}

	public static class MathWin32
	{
		public static void DrawCircle(Graphics G, Pen P, V3D C, double R)
		{
			G.DrawEllipse(P, (float)(C.X - R), (float)(C.Y - R), (float)R * 2, (float)R * 2);
		}
		public static void DrawCircle(Graphics G, Pen P, V3D[] Pts)
		{
			Line3D A = new Line3D((Pts[0] + Pts[1]) / 2, (Pts[0] + Pts[1]) / 2 + ((Pts[1] - Pts[0]).RotZ(90)));
			Line3D B = new Line3D((Pts[1] + Pts[2]) / 2, (Pts[1] + Pts[2]) / 2 + ((Pts[2] - Pts[1]).RotZ(90)));
			V3D X = A * B;
			Double R = (Pts[0] - X).Length;
			DrawCircle(G, P, X, R);
		}
		public static void FillCircle(Graphics G, Brush Br, V3D C, double R)
		{
			G.FillEllipse(Br, (float)(C.X - R), (float)(C.Y - R), (float)R * 2, (float)R * 2);
		}
		public static void FillCircle(Graphics G, Brush Br, V3D[] Pts)
		{
			Line3D A = new Line3D((Pts[0] + Pts[1]) / 2, (Pts[0] + Pts[1]) / 2 + ((Pts[1] - Pts[0]).RotZ(90)));
			Line3D B = new Line3D((Pts[1] + Pts[2]) / 2, (Pts[1] + Pts[2]) / 2 + ((Pts[2] - Pts[1]).RotZ(90)));
			V3D X = A * B;
			Double R = (Pts[0] - X).Length;
			FillCircle(G, Br, X, R);
		}
		//public static void DrawArc(Graphics G,Pen P,V3D C,double R,double StartAngle,double Endangle)
		public static void DrawArc(Graphics G, Pen P, V3D[] Pts)
		{
			Line3D RA = new Line3D((Pts[0] + Pts[1]) / 2, (Pts[0] + Pts[1]) / 2 + ((Pts[1] - Pts[0]).RotZ(90)));
			Line3D RB = new Line3D((Pts[1] + Pts[2]) / 2, (Pts[1] + Pts[2]) / 2 + ((Pts[2] - Pts[1]).RotZ(90)));

			V3D X = RA * RB;
			Double R = (Pts[0] - X).Length;

			V3D A = Pts[0] - X;
			double startangle = V3D.Angle360D(V3D.Red, A, V3D.Blue);
			V3D B = Pts[1] - X;
			V3D C = Pts[2] - X;
			double endangle = V3D.Angle360D(A, C, V3D.Blue);
			if (V3D.Angle360D(A, B, V3D.Blue) > endangle)
				endangle -= 360;

			G.DrawArc(P, (float)(X.X - R), (float)(X.Y - R), (float)R * 2, (float)R * 2, -(float)startangle, -(float)endangle);
			//for (int i = 0; i < 3; i++)
			//	Pts[i].DrawGraphics(G, Brushes.Lime, 3);
		}

		public static Color ColorGrad(Color C1, Color C2, double N)
		{
			double A = C1.A * (1-N ) + C2.A * N;
			double R = C1.R * (1-N ) + C2.R * N;
			double G = C1.G * (1-N ) + C2.G * N;
			double B = C1.B * (1-N ) + C2.B * N;
			return Color.FromArgb((byte)A, (byte)R, (byte)G, (byte)B);
		}
	}
	//V3D_Geometry
	/*
	public class VPolygon
	{
		public V3D[] Points;
	}
	public class VMesh
	{
		public struct MeshTri
		{
			public int P1, P2, P3;
			public V3D C1, C2, C3;
		}
		public V3D AABB_L;
		public V3D AABB_H;
		public V3D[] Points;
		public MeshTri[] Triangles;
		//Constructors
		public VMesh()
		{
			Points = new V3D[0];
		}
		public void BuildBox(V3D Size)
		{
			int i;
			int[] Pts = new int[36]
				{0,1,2,0,2,3,
				0,1,4,1,4,5,
				1,2,5,2,5,6,
				2,3,6,3,6,7,
				3,0,7,0,7,4,
				4,5,6,4,6,7};
			Points = new V3D[8];
			Triangles = new MeshTri[12];
			Points[0] = new V3D(-Size.X / 2.0f, -Size.Y / 2.0f, -Size.Z / 2.0f);
			Points[1] = new V3D(-Size.X / 2.0f, -Size.Y / 2.0f, Size.Z / 2.0f);
			Points[2] = new V3D(-Size.X / 2.0f, Size.Y / 2.0f, Size.Z / 2.0f);
			Points[3] = new V3D(-Size.X / 2.0f, Size.Y / 2.0f, -Size.Z / 2.0f);
			Points[4] = new V3D(Size.X / 2.0f, -Size.Y / 2.0f, -Size.Z / 2.0f);
			Points[5] = new V3D(Size.X / 2.0f, -Size.Y / 2.0f, Size.Z / 2.0f);
			Points[6] = new V3D(Size.X / 2.0f, Size.Y / 2.0f, Size.Z / 2.0f);
			Points[7] = new V3D(Size.X / 2.0f, Size.Y / 2.0f, -Size.Z / 2.0f);
			i = 0;
			foreach (MeshTri Tr in Triangles)
			{
				Triangles[i].P1 = Pts[i * 3];
				Triangles[i].P2 = Pts[i * 3 + 1];
				Triangles[i].P3 = Pts[i * 3 + 2];
				Triangles[i].C1 = V3D.White;
				Triangles[i].C2 = V3D.White;
				Triangles[i].C3 = V3D.White;
				i++;
			}
			AABB();
		}
		public void AABB()
		{
			if (Points.Length == 0)
			{
				AABB_L = V3D.Null;
				AABB_H = V3D.Null;
			}
			else
			{
				AABB_L = Points[0];
				AABB_H = Points[0];
				foreach (V3D P in Points)
				{
					AABB_L = new V3D(Math.Min(AABB_L.X, P.X), Math.Min(AABB_L.Y, P.Y), Math.Min(AABB_L.Z, P.Z));
					AABB_H = new V3D(Math.Max(AABB_L.X, P.X), Math.Max(AABB_L.Y, P.Y), Math.Max(AABB_L.Z, P.Z));
				}
			}
		}
		
		public void Offset(V3D v)
		{
			for (int i = 0; i < Points.Length; i++)
				Points[i] += v;
		}
		//OpenGL
		public void DrawLine()
		{
			foreach (MeshTri Tri in Triangles)
			{
				V3D P1 = Points[Tri.P1];
				V3D P2 = Points[Tri.P2];
				V3D P3 = Points[Tri.P3];
				GL.glBegin(GL.GL_LINE_LOOP);
				Tri.C1.GLCol();
				P1.GLDraw();
				Tri.C2.GLCol();
				P2.GLDraw();
				Tri.C3.GLCol();
				P3.GLDraw();
				GL.glEnd();
			}
		}
		public void DrawExp()
		{
			GL.glBegin(GL.GL_TRIANGLES);
			foreach (MeshTri Tri in Triangles)
			{
				V3D P1 = Points[Tri.P1];
				V3D P2 = Points[Tri.P2];
				V3D P3 = Points[Tri.P3];
				float w1 = 0.25f, w2 = 0.5f;
				Tri.C1.GLCol();
				(w2 * P1 + w1 * P2 + w1 * P3).GLDraw();
				Tri.C2.GLCol();
				(w1 * P1 + w2 * P2 + w1 * P3).GLDraw();
				Tri.C3.GLCol();
				(w1 * P1 + w1 * P2 + w2 * P3).GLDraw();
			}
			GL.glEnd();
		}
		public void DrawDebug()
		{
			foreach (MeshTri Tri in Triangles)
			{
				V3D P1 = Points[Tri.P1];
				V3D P2 = Points[Tri.P2];
				V3D P3 = Points[Tri.P3];
				Tri.C1.GLCol();
				P1.GLSign(0.1);
				Tri.C2.GLCol();
				P2.GLSign(0.1);
				Tri.C3.GLCol();
				P3.GLSign(0.1);
				GL.glBegin(GL.GL_LINE_LOOP);
				Tri.C1.GLCol();
				P1.GLDraw();
				Tri.C2.GLCol();
				P2.GLDraw();
				Tri.C3.GLCol();
				P3.GLDraw();
				GL.glEnd();
			}
		}
		
		//Operations
		public bool PointIn(V3D P)
		{
			int i = 0;
			V3D P2 = AABB_H + V3D.White;
			foreach (MeshTri Tri in Triangles)
			{
				V3D M = new VLine(P2, P).IntersectTriangleS(new VTri(Points[Tri.P1], Points[Tri.P2], Points[Tri.P3]));
				if (M != null)
					i++;
			}
			return ((i % 2) == 1);
		}
		public void Merge(VMesh M)
		{
			for (int i = 0; i < Triangles.Length; i++)
			{
				if (M.PointIn(Points[Triangles[i].P1]))
					Triangles[i].C1 = V3D.Green;
				else
					Triangles[i].C1 = V3D.Blue;
				if (M.PointIn(Points[Triangles[i].P2]))
					Triangles[i].C2 = V3D.Green;
				else
					Triangles[i].C2 = V3D.Blue;
				if (M.PointIn(Points[Triangles[i].P3]))
					Triangles[i].C3 = V3D.Green;
				else
					Triangles[i].C3 = V3D.Blue;
			}
		}
		
	*/


	public class PT1
	{
		double T;
		double K;
		public double A;
		public void SetTime(double t)
		{
			T = t;
		}
		public double Value;

		public PT1(double tparam, double kparam, double aparam)
		{
			T = tparam;
			K = kparam;
			A = Value = aparam;
		}
		public override String ToString()
		{
			return Value + "->" + A;
		}
		public void Set(double V)
		{
			A = V;
		}
		public double Process(double dt)
		{
			Value = Value * (1 - 1 / (T / dt)) + K / (T / dt) * A;
			return Value;
		}
		public double Process(double dt, double NewA)
		{
			Set(NewA);
			return Process(dt);
		}
		public static void Swap(ref PT1 A, ref PT1 B)
		{
			PT1 Temp = A;
			A = B;
			B = Temp;
		}
	}
	public class PT1_V3D
	{
		double T;
		double K;
		public V3D A;

		public V3D Value;

		public PT1_V3D(double tparam, double kparam, V3D aparam)
		{
			T = tparam;
			K = kparam;
			A = Value = aparam;
		}
		public void Set(V3D V)
		{
			A = V;
		}
		public V3D Process(double dt)
		{
			PT1 P;
			P = new PT1(T, K, Value.X);
			Value.X = P.Process(dt, A.X);
			P = new PT1(T, K, Value.Y);
			Value.Y = P.Process(dt, A.Y);
			P = new PT1(T, K, Value.Z);
			Value.Z = P.Process(dt, A.Z);
			return Value;
		}
		public V3D Process(double dt, V3D NewA)
		{
			Set(NewA);
			return Process(dt);
		}
		public static void Swap(ref PT1_V3D A, ref PT1_V3D B)
		{
			PT1_V3D Temp = A;
			A = B;
			B = Temp;
		}
	}
	public class PT1_V3G
	{
		double T;
		double K;
		public V3G A;

		public V3G Value;

		public PT1_V3G(double tparam, double kparam, V3G aparam)
		{
			T = tparam;
			K = kparam;
			A = Value = aparam;
		}
		public void Set(V3G V)
		{
			A = V;
		}
		public V3D Process(double dt)
		{
			PT1 P;
			P = new PT1(T, K, Value.Longitude);
			Value.Longitude = P.Process(dt, A.Longitude);
			P = new PT1(T, K, Value.Latitude);
			Value.Latitude = P.Process(dt, A.Latitude);
			P = new PT1(T, K, Value._Height);
			Value._Height = P.Process(dt, A._Height);
			return Value;
		}
		public V3D Process(double dt, V3D NewA)
		{
			Set(NewA);
			return Process(dt);
		}
		public static void Swap(ref PT1_V3D A, ref PT1_V3D B)
		{
			PT1_V3D Temp = A;
			A = B;
			B = Temp;
		}
	}
	public class PI
	{
		double T;
		double K;
		public double A;
		public double Value;
		public PI(double tparam, double kparam, double aparam)
		{
			T = tparam;
			K = kparam;
			A = Value = aparam;
		}
		public override String ToString()
		{
			return Value + "->" + A;
		}
		public void Set(double V)
		{
			A = V;
		}
		public double Process(double dt)
		{
			Value = Value + K / (T / dt) * A;
			return Value;
		}
		public double Process(double dt, double NewA)
		{
			Set(NewA);
			return Process(dt);
		}
		public static void Swap(ref PI A, ref PI B)
		{
			PI Temp = A;
			A = B;
			B = Temp;
		}
	}
	public class Damper
	{
		public double L0;
		public double L;
		double Lt;
		double dl;

		double M;

		double D;
		double C;

		double F;

		double N;
		public Damper(double l0, double m, double d, double c)
		{
			L0 = L = Lt = l0;
			M = m;
			D = d;
			C = c;
			F = 0;
			double DD = Math.Max(D, C);
			N = 1;
			while (DD > 100)
			{
				DD /= 10;
				N *= 10;
			}
		}
		public void Process(double dt)
		{
			for (int i = 0; i < N; i++)
			{
				dl += (F - C * ((L - Lt) / (dt / N)) - D * (L - L0)) / M * dt / N;
				Lt = L;
				L += dl * dt / N;
			}
		}
		public double Force
		{
			get
			{
				return D * (L - L0);
			}
		}
	}
	public class DamperF
	{
		double L0;
		public double L;
		double Lt;
		double Lt2;
		double dl;
		double al;
		double M;

		public double D;
		public double C;

		double force;

		public DamperF(double l0, double m, double d, double c)
		{
			L0 = L = Lt = Lt2 = l0;
			dl = al = 0;
			M = m;
			D = d;
			C = c;
			force = 0;
		}
		public void Process(double dt, double NewL)
		{
			Lt2 = Lt;
			Lt = L;
			L = NewL;
			dl = (L - Lt) / dt;
			al = (dl - (Lt - Lt2) / dt);
			//al = (F - C * (dl / dt) - D * (L - L0)) / M;
			force = al * M + C * dl + D * (L - L0);
		}
		public void Process(double dt)
		{
			Process(dt, L);
		}
		public double Force
		{
			get
			{
				return force;
			}
		}
	}
}